#include <opencv2/opencv.hpp>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"


class ManagerNode : public rclcpp::Node {

public:
    ManagerNode(const rclcpp::NodeOptions &options) : Node("manager_node", options) {
        RCLCPP_INFO(this->get_logger(), "节点已启动：%s", this->get_name());
        image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/image_raw", 1,
            [this](sensor_msgs::msg::Image::SharedPtr msg) { this->imageCallback(msg); }
        );
    }

private:
    void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
        try {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
            auto img = cv_ptr->image;
            cv::imshow("Camera Image", img);
            cv::waitKey(1);
        } catch (cv_bridge::Exception &e) {
            RCLCPP_ERROR(this->get_logger(), "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;

};

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(ManagerNode)
